#include "fpal.hpp"

// -----------------------------------------------------------
// 封装在命名空间 fpal 中
// -----------------------------------------------------------
namespace fpal {

// --- 复杂运算集合 - 实现 (非模板函数) ---
// 使用定制迭代算法，NUM_ITERATIONS=5
fp_coord_t sqrt(fp_coord_t val) {
    return custom_sqrt_iterative<fp_coord_t, 5>(val);
}
fp_vec_unit_t sqrt(fp_vec_unit_t val) {
    return custom_sqrt_iterative<fp_vec_unit_t, 5>(val);
}

// 倒数平方根 (Inverse Sqrt)
fp_coord_t inv_sqrt(fp_coord_t val) {
    return custom_inv_sqrt_iterative<fp_coord_t, 5>(val);
}
fp_vec_unit_t inv_sqrt(fp_vec_unit_t val) {
    return custom_inv_sqrt_iterative<fp_vec_unit_t, 5>(val);
}

// -----------------------------------------------------------
// 3. 线性代数运算集合 - 实现
// -----------------------------------------------------------

// 向量加法
vec3_fp_coord vec3_add(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS INLINE
    vec3_fp_coord res;
    res.x = add(v1.x, v2.x);
    res.y = add(v1.y, v2.y);
    res.z = add(v1.z, v2.z);
    return res;
}
vec3_fp_vec_unit vec3_add(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS INLINE
    vec3_fp_vec_unit res;
    res.x = add(v1.x, v2.x);
    res.y = add(v1.y, v2.y);
    res.z = add(v1.z, v2.z);
    return res;
}

// 向量减法
vec3_fp_coord vec3_sub(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS INLINE
    vec3_fp_coord res;
    res.x = sub(v1.x, v2.x);
    res.y = sub(v1.y, v2.y);
    res.z = sub(v1.z, v2.z);
    return res;
}
vec3_fp_vec_unit vec3_sub(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS INLINE
    vec3_fp_vec_unit res;
    res.x = sub(v1.x, v2.x);
    res.y = sub(v1.y, v2.y);
    res.z = sub(v1.z, v2.z);
    return res;
}

// 点积
fp_vec_unit_t vec3_dot(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS INLINE
    return add(add(mul(v1.x, v2.x), mul(v1.y, v2.y)), mul(v1.z, v2.z));
}
fp_coord_t vec3_dot(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS INLINE
    return add(add(mul(v1.x, v2.x), mul(v1.y, v2.y)), mul(v1.z, v2.z));
}

// 叉积
vec3_fp_vec_unit vec3_cross(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS INLINE
    vec3_fp_vec_unit res;
    res.x = sub(mul(v1.y, v2.z), mul(v1.z, v2.y));
    res.y = sub(mul(v1.z, v2.x), mul(v1.x, v2.z));
    res.z = sub(mul(v1.x, v2.y), mul(v1.y, v2.x));
    return res;
}
vec3_fp_coord vec3_cross(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS INLINE
    vec3_fp_coord res;
    res.x = sub(mul(v1.y, v2.z), mul(v1.z, v2.y));
    res.y = sub(mul(v1.z, v2.x), mul(v1.x, v2.z));
    res.z = sub(mul(v1.x, v2.y), mul(v1.y, v2.x));
    return res;
}

// 向量长度平方
fp_vec_unit_t vec3_length_sq(vec3_fp_vec_unit v) {
    #pragma HLS INLINE
    return vec3_dot(v, v);
}
fp_coord_t vec3_length_sq(vec3_fp_coord v) {
    #pragma HLS INLINE
    return vec3_dot(v, v);
}

// 向量长度
fp_vec_unit_t vec3_length(vec3_fp_vec_unit v) {
    #pragma HLS INLINE
    return sqrt(vec3_length_sq(v));
}
// 额外重载，以支持直接传入分量
fp_coord_t vec3_length(fp_coord_t x, fp_coord_t y, fp_coord_t z) {
    #pragma HLS INLINE
    return sqrt(add(add(mul(x,x), mul(y,y)), mul(z,z)));
}
fp_coord_t vec3_length(vec3_fp_coord v) {
    #pragma HLS INLINE
    return sqrt(vec3_length_sq(v));
}

// 向量归一化
vec3_fp_vec_unit vec3_normalize(vec3_fp_vec_unit v) {
    #pragma HLS INLINE
    fp_vec_unit_t len_sq = vec3_length_sq(v);
    if (len_sq < (fp_vec_unit_t)0.00001) 
        return {(fp_vec_unit_t)0,(fp_vec_unit_t)0,(fp_vec_unit_t)0};
    
    fp_vec_unit_t inv_len = inv_sqrt(len_sq);
    vec3_fp_vec_unit res;
    res.x = mul(v.x, inv_len);
    res.y = mul(v.y, inv_len);
    res.z = mul(v.z, inv_len);
    return res;
}
vec3_fp_coord vec3_normalize(vec3_fp_coord v) {
    #pragma HLS INLINE
    fp_coord_t len_sq = vec3_length_sq(v);
    if (len_sq < (fp_coord_t)0.00001) 
        return {(fp_coord_t)0,(fp_coord_t)0,(fp_coord_t)0};
    
    fp_coord_t inv_len = inv_sqrt(len_sq);
    vec3_fp_coord res;
    res.x = mul(v.x, inv_len);
    res.y = mul(v.y, inv_len);
    res.z = mul(v.z, inv_len);
    return res;
}

// 矩阵-向量乘法 (4x4 矩阵 * 4元素向量)
vec3_fp_coord mat4_vec4_mul(const fp_coord_t mat[4][4], vec3_fp_coord vec_xyz, fp_coord_t w_in) {
    #pragma HLS INLINE
    // 保持完全分区和内联，因为 DSP 资源充足，且顶点处理需要高性能
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=1
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=2
    
    vec3_fp_coord res;
    
    res.x = add(add(mul(mat[0][0], vec_xyz.x), mul(mat[0][1], vec_xyz.y)), 
                add(mul(mat[0][2], vec_xyz.z), mul(mat[0][3], w_in)));
    res.y = add(add(mul(mat[1][0], vec_xyz.x), mul(mat[1][1], vec_xyz.y)),
                add(mul(mat[1][2], vec_xyz.z), mul(mat[1][3], w_in)));
    res.z = add(add(mul(mat[2][0], vec_xyz.x), mul(mat[2][1], vec_xyz.y)),
                add(mul(mat[2][2], vec_xyz.z), mul(mat[2][3], w_in)));
    
    // 注意：这里没有处理 w 分量归一化，通常在投影后进行透视除法。
    // 如果需要透视除法，应在外部调用此函数后手动进行。
    return res;
}


// 矩阵-向量乘法 (3x3 矩阵 * 3元素向量)
vec3_fp_vec_unit mat3_vec3_mul(const fp_vec_unit_t mat[3][3], vec3_fp_vec_unit vec) {
    #pragma HLS INLINE
    // 保持完全分区和内联，因为 DSP 资源充足，且顶点处理需要高性能
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=1
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=2

    vec3_fp_vec_unit res;
    res.x = add(add(mul(mat[0][0], vec.x), mul(mat[0][1], vec.y)), mul(mat[0][2], vec.z));
    res.y = add(add(mul(mat[1][0], vec.x), mul(mat[1][1], vec.y)), mul(mat[1][2], vec.z));
    res.z = add(add(mul(mat[2][0], vec.x), mul(mat[2][1], vec.y)), mul(mat[2][2], vec.z));
    return res;
}

// -----------------------------------------------------------
// 4. 定点数类型转换函数 - 实现
// -----------------------------------------------------------

// 将 fp_coord_t 转换为 fp_vec_unit_t
fp_vec_unit_t conv_coord_to_vec_unit(fp_coord_t val) {
    #pragma HLS INLINE
    return (fp_vec_unit_t)val;
}

// 将 fp_vec_unit_t 转换为 fp_coord_t
fp_coord_t conv_vec_unit_to_coord(fp_vec_unit_t val) {
    #pragma HLS INLINE
    return (fp_coord_t)val;
}

// 将 fp_vec_unit_t 转换为 fp_color_channel_t (0-255)
fp_color_channel_t conv_fp_to_color_channel(fp_vec_unit_t val) {
    #pragma HLS INLINE
    fp_vec_unit_t saturated_val = saturate(val); // 先饱和到 [0, 1]
    
    // 使用 fp_color_scale_t 进行缩放，避免 fp_vec_unit_t 的 I=3 范围溢出
    fp_color_scale_t scaled_val = (fp_color_scale_t)saturated_val * (fp_color_scale_t)255.0; 
    
    // 钳制到 [0, 255] 范围
    fp_color_scale_t clamped_scaled_val = clamp(scaled_val, (fp_color_scale_t)0.0, (fp_color_scale_t)255.0);

    // 调用四舍五入辅助函数
    return round_to_nearest_uint8(clamped_scaled_val);
}

// 将 fp_coord_t 转换为 fp_color_channel_t (0-255)
fp_color_channel_t conv_fp_to_color_channel(fp_coord_t val) {
    #pragma HLS INLINE
    fp_coord_t saturated_val = clamp(val, (fp_coord_t)0.0, (fp_coord_t)1.0); // 先钳制到 [0, 1]
    
    // fp_coord_t (I=18) 可以直接容纳 255.0，不会溢出
    fp_coord_t scaled_val = saturated_val * (fp_coord_t)255.0;
    
    // 钳制到 [0, 255] 范围
    fp_coord_t clamped_scaled_val = clamp(scaled_val, (fp_coord_t)0.0, (fp_coord_t)255.0);

    // 调用四舍五入辅助函数
    return round_to_nearest_uint8(clamped_scaled_val);
}

} // namespace fpal


// -----------------------------------------------------------
// 5. HLS 顶层函数定义 (全局命名空间)
// -----------------------------------------------------------
void fpal_wrapper(
    fpal::fp_coord_t in_coord_x,
    fpal::fp_coord_t in_coord_y,
    fpal::fp_coord_t in_coord_z,
    fpal::fp_vec_unit_t in_vec_u,
    fpal::fp_vec_unit_t in_vec_v,
    fpal::fp_color_channel_t &out_color_r,
    fpal::fp_coord_t &out_coord_len_sq,
    fpal::fp_vec_unit_t &out_vec_norm_x
) {
    // HLS 接口指令：将所有端口映射到 AXI-Lite 总线
    #pragma HLS INTERFACE s_axilite port=return
    #pragma HLS INTERFACE s_axilite port=in_coord_x
    #pragma HLS INTERFACE s_axilite port=in_coord_y
    #pragma HLS INTERFACE s_axilite port=in_coord_z
    #pragma HLS INTERFACE s_axilite port=in_vec_u
    #pragma HLS INTERFACE s_axilite port=in_vec_v
    #pragma HLS INTERFACE s_axilite port=out_color_r
    #pragma HLS INTERFACE s_axilite port=out_coord_len_sq
    #pragma HLS INTERFACE s_axilite port=out_vec_norm_x

    // 对顶层函数进行流水线化，以提高吞吐量。
    // HLS将尝试实现一个II=1的流水线，如果无法达到，会报告并给出更高的II。
    #pragma HLS PIPELINE II=1 

    // 示例：直接使用 FPAL 命名空间中的函数和类型
    fpal::vec3_fp_coord test_coord_vec = {in_coord_x, in_coord_y, in_coord_z};
    // 注意：in_vec_u, in_vec_v 现在是 fp_vec_unit_t，(fpal::fp_vec_unit_t)0.5 也是同类型。
    fpal::vec3_fp_vec_unit test_unit_vec = {in_vec_u, in_vec_v, (fpal::fp_vec_unit_t)0.5};

    // 计算坐标向量的长度平方
    out_coord_len_sq = fpal::vec3_length_sq(test_coord_vec);

    // 归一化单位向量并获取其X分量
    fpal::vec3_fp_vec_unit normalized_vec = fpal::vec3_normalize(test_unit_vec);
    out_vec_norm_x = normalized_vec.x;

    // 将归一化向量的Y分量转换为颜色通道 (0-255)
    out_color_r = fpal::conv_fp_to_color_channel(normalized_vec.y);
}
